'''
This demo show the communication interface of MR813 motion control board based on Lcm
- robot_control_cmd_lcmt.py
- cyberdog2_ctrl.toml
'''
import lcm
import toml
import sys
import os
import time

from robot_control_cmd_lcmt import robot_control_cmd_lcmt

def findAllFile(base):
    # 遍历指定目录下的所有文件
    for root, ds, fs in os.walk(base):
        for f in fs:
            yield f

def main():
    base = './'  # 当前目录
    num = 0
    filelist = []
    # 列出当前目录下的所有文件
    for i in findAllFile(base):
        filelist.append(i)
        print(str(num) + "," + str(filelist[num]))
        num = num + 1
    print('Input a toml ctrl file number:')  # 提示用户输入文件编号
    numInput = int(input())

    # 初始化 LCM 客户端
    lc = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
    msg = robot_control_cmd_lcmt()  # 创建控制命令消息对象
    file = os.path.join(base, filelist[numInput])  # 获取选择的文件路径
    print("Load file=%s\n" % file)  # 打印加载的文件名

    try:
        # 加载配置文件
        steps = toml.load(file)
        # 遍历配置文件中的每个步骤
        for step in steps['step']:
            msg.mode = step['mode']  # 设置控制模式
            msg.value = step['value']
            msg.contact = step['contact']
            msg.gait_id = step['gait_id']  # 设置步态 ID
            msg.duration = step['duration']  # 设置期望执行时间
            msg.life_count += 1  # 更新生命计数，确保命令生效

            # 设置速度、姿态、位置等参数
            for i in range(3):
                msg.vel_des[i] = step['vel_des'][i]
                msg.rpy_des[i] = step['rpy_des'][i]
                msg.pos_des[i] = step['pos_des'][i]
                msg.acc_des[i] = step['acc_des'][i]
                msg.acc_des[i+3] = step['acc_des'][i+3]
                msg.foot_pose[i] = step['foot_pose'][i]
                msg.ctrl_point[i] = step['ctrl_point'][i]
            for i in range(2):
                msg.step_height[i] = step['step_height'][i]

            # 发送控制命令
            lc.publish("robot_control_cmd", msg.encode())
            print('robot_control_cmd lcm publish mode :', msg.mode, "gait_id :", msg.gait_id, "msg.duration=", msg.duration)
            time.sleep(0.1)  # 短暂休眠

        # 发送心跳信号，保持通信
        for i in range(300):  # 60 秒心跳信号
            lc.publish("robot_control_cmd", msg.encode())
            time.sleep(0.2)  # 每 0.2 秒发送一次

    except KeyboardInterrupt:
        # 捕获键盘中断，发送纯阻尼模式命令
        msg.mode = 7  # PureDamper
        msg.gait_id = 0
        msg.duration = 0
        msg.life_count += 1
        lc.publish("robot_control_cmd", msg.encode())
        pass
    sys.exit()  # 退出程序

# 主函数
if __name__ == '__main__':
    main()